/*
 * @Author: your name
 * @Date: 2021-07-17 09:39:26
 * @LastEditTime: 2021-11-02 10:07:02
 * @LastEditors: Mcfly
 * @Description: In User Settings Edit
 * @FilePath: \FTTfly\Ctrl\PID.c
 */
#include "PID.h"
#include "stdlib.h"
#include "ti_system.h"

PIDctrler *initPID(float Kp, float Ki, float Kd, float limitI, float deltaT)
{
    PIDctrler *pid = (PIDctrler *)malloc(sizeof(PIDctrler));
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->limitI = limitI;
    pid->integral = 0;
    pid->lastError = 0;
    pid->deltaT = deltaT;
	
    pid->pOut = 0;
    pid->iOut = 0;
    pid->dOut = 0;
    pid->out = 0;
    return pid;
}

float PIDProcess(PIDctrler *pid, float error)
{
    pid->integral += error;
    pid->integral = LIMIT(-pid->limitI, pid->limitI, pid->integral);
	
	pid->pOut = error * pid->Kp;
    pid->iOut = pid->integral * pid->Ki * pid->deltaT;
    pid->dOut = (error - pid->lastError) * pid->Kd / pid->deltaT;
	
    pid->out = pid->pOut + pid->iOut + pid->dOut;
	
    pid->lastError = error;
	
    return pid->out;
}

float PIDProcessWithSelfIntegralErr(PIDctrler *pid, float error, float integralErr)
{
    pid->integral += integralErr;
    pid->integral = LIMIT(-pid->limitI, pid->limitI, pid->integral);
	
	pid->pOut = error * pid->Kp;
    pid->iOut = pid->integral * pid->Ki * pid->deltaT;
    pid->dOut = (error - pid->lastError) * pid->Kd / pid->deltaT;
	
    pid->out = pid->pOut + pid->iOut + pid->dOut;
	
    pid->lastError = error;
	
    return pid->out;
}

float PIDProcessWithSelfIntegralErrAndFilterD(PIDctrler *pid, float error, float integralErr, ButterBufferData *filterBuf, ButterParameter *filterD)
{
    pid->integral += integralErr;
    pid->integral = LIMIT(-pid->limitI, pid->limitI, pid->integral);
	
	pid->pOut = error * pid->Kp;
    pid->iOut = pid->integral * pid->Ki * pid->deltaT;
    pid->dOut = (error - pid->lastError) * pid->Kd / pid->deltaT;
	
    pid->dOut = ButterWorth_Work_LPB(pid->dOut, filterBuf, filterD);

    pid->out = pid->pOut + pid->iOut + pid->dOut;
	
    pid->lastError = error;
	
    return pid->out;
}

float PIDProcessWithDiffTriErr(PIDctrler *pid, float pErr, float integralErr, float dErr)
{
    pid->integral += integralErr;
    pid->integral = LIMIT(-pid->limitI, pid->limitI, pid->integral);
	
	pid->pOut = pErr * pid->Kp;
    pid->iOut = pid->integral * pid->Ki * pid->deltaT;
    pid->dOut = (dErr - pid->lastError) * pid->Kd / pid->deltaT;
	
    pid->out = pid->pOut + pid->iOut + pid->dOut;
	
    pid->lastError = dErr;
	
    return pid->out;
}

void PIDctrler_SetKp(PIDctrler *pid, float Kp)
{
    pid->Kp = Kp;
}
void PIDctrler_SetKi(PIDctrler *pid, float Ki)
{
    pid->Ki = Ki;
}
void PIDctrler_SetKd(PIDctrler *pid, float Kd)
{
    pid->Kd = Kd;
}
